#include <chassis_controller/odometry_compensation.h>

DynamicOdom::DynamicOdom()
{
}

double DynamicOdom::getFactor(double load)
{   
    if (abs(max_load_ - min_load_)<0.0001){
        return 1.0;
    }
    k_ = (max_factor_ - min_factor_)/(max_load_ - min_load_);
    offset_ = (max_load_ * min_factor_ - min_load_ * max_factor_)/(max_load_ - min_load_);
    return load * k_ + offset_;
}
